/*
panasonic.c
designed pansonic motor driver drive,control motor action , config and init
designed by lunantng

------------------------
2020-04-02 10:16:44
create
------------------------
*/
#include "panasonic.h"
#include "aos/kernel.h"
#include "stdlib.h"

int xp_pans_write_coil(const PANS_handler *handler,u16 addr,PANS_type state);
// static PANS_state pans_state={0};

/*
motor driver init
by 485 modbus
param:
    port:uart port
    speed:uart speed
return:
    0=success
    -1:modbus init fail
    -2=motor deiver config fail
*/
int xp_pans_init(PANS_handler *state){
    int ret=0;

    state->init=0;
    state->run=1;
    state->en=0;
    if(state->slave_id<2||state->slave_id>127){
        println("warning:input slave id wrong! slave_id=%d",state->slave_id);
        state->slave_id=127;
    }
    
    ret=xp_modbus_init(state->port,state->speed,MB_PAR_NONE,100);
    if(ret!=0){
        // println("pansonic motor modbus init fail ! xp_modbus_init() return=%d",ret);
        return -1;
    }
    aos_msleep(10);
    // xp_pans_clear_location(state);
    if(xp_pans_clear_warn(state)!=0)return -2;
    if(xp_pans_en(state,pans_off)!=0)return -3;
    aos_msleep(100);
    if(xp_pans_en(state,pans_on)!=0)return -4;;
    if(xp_pans_run(state,pans_off)!=0)return -5;
    // xp_pans_write_coil(state,0x61,pans_on);
    // xp_pans_write_coil(state,0x61,pans_off);
    // xp_pans_write_coil(state,0x122,pans_off);
    //config
    if(xp_pans_config(state)!=0)return -6;
    state->init=1;
    // ret=xp_pans_en(state,pans_on);
    // if(ret!=0)return -3;
    return 0;
}

/*
pansonic motor config
param:
    state:pans state
return:
    0=success
    -1=setting home offset fail
    -2=setting max limit fail
    -3=setting min limit fail
    -4=setting speeds fail
    -5=setting acc fail
    -6=setting dec fail
*/
int xp_pans_config(PANS_handler *state){
    int ret=0;
    MB_data modbus;
    u8 rec_data[10];
    u8 rec_len;
    

    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    //home offset setting
    modbus.mb_fun=mb_hold_w;
    modbus.rwLen=2;
    modbus.slave_id=state->slave_id;
    // modbus.start_addr=0x4631;
    // modbus.wBuf=&state->config.home_offset;
    // ret=xp_modbus_ask(state->port,&modbus);
    // if(ret!=0)return -1;
    //max limit setting
    modbus.start_addr=0x4633;
    modbus.wBuf=(u16 *)&state->config.max_limit;
    xp_modbus_ask(state->port,&modbus);
    //min limit setting
    modbus.start_addr=0x4635;
    modbus.wBuf=(u16 *)&state->config.min_limit;
    xp_modbus_ask(state->port,&modbus);
    //speed setting
    if(xp_pans_config_speed(state)!=0)return -10;
    return 0;
}


/***********************************coil read/write*************************/
/*
pansonic motor set coil
param:
    handler:object 
    addr:reginster address
    state:PANS_en_type
return:
    0=success
    -1:modbus return error
*/
static int xp_pans_write_coil(const PANS_handler *handler,u16 addr,PANS_type state){
    int sta=0;
    u16 val;
    MB_data modbus;
    //保证多线程通信的可靠性
    u8 rec_data[10];
    u8 rec_len;
    u8 count=5;

    if(state==pans_off){
        val=0;
    }
    else{
        val=0xff00;
    }
    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_coil_w;
    modbus.start_addr=addr;
    modbus.fun_code=0x05;
    modbus.wBuf=&val;
    modbus.rwLen=1;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--){
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta!=0)return -1;
}

/*
pansonic motor read coil
param；
    handler:motor object
    addr:reginster address
return:coil state,pans_en_type
*/
static PANS_type xp_pans_read_coil(const PANS_handler *handler,u16 addr){
    int sta=0;
    MB_data modbus;
    u8 rec_data[10];
    u8 rec_len;
    u16 val;
    u8 count=5;

    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_coil_r;
    modbus.start_addr=addr;
    modbus.rwLen=1;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--){
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)break;
    }
    if(sta!=0)return pans_error;
    val=rec_data[0];
    // println("read len=%d,data=%d",rec_len,rec_data[0]);
    if(val==0)return pans_off;
    else if(val==1)return pans_on;
    else return pans_error;
}

/*
pansonic motor enable state set
param:
    state:motor handler
    en:0=disable,1=enable
return:
    0=success
    1:need't action
    -1=modbus return error
*/
int xp_pans_en(PANS_handler *handler,PANS_type en){
    if(handler->en!=en){
        handler->en=en;
    }
    return xp_pans_write_coil(handler,PANS_EN_RES,en);
}



/*
panasonic motor run set
param:
    hander:motor handler
    run:run state
return:
    0=success
    1=need't action
    -1=modbus return error
*/
int xp_pans_run(PANS_handler *handler,PANS_type run){
    if(handler->run!=run){
        handler->run=run;
    }
    return xp_pans_write_coil(handler,PANS_RUN_RES,run);
}


/*
pansonic morto back home
param:
    hander:motor handler
return:
    0=success
    -1=modbus return error
*/
int xp_pans_gohome(const PANS_handler *handler,PANS_type go){

    return xp_pans_write_coil(handler,PANS_HOME_RES,go);
}


/*
panasonic motor fast stop
param:
    handler:motor object
    addr:reginster address
    stop:stop type
return:
    0=success
    -1:modbus return error
*/
int xp_pans_fast_stop(const PANS_handler *handler,PANS_type stop){

    return xp_pans_write_coil(handler,PANS_H_STOP_RES,stop);
}


/*
pansonic motor slow stop
param:
    handler:motor object
    addr:reginster address
    stop:stop type
return:
    0=success
    -1:modbus return error
*/
int xp_pans_slow_stop(const PANS_handler *handler,PANS_type stop){
    return xp_pans_write_coil(handler,PANS_S_STOP_RES,stop);
}



/*
get motor en state
param:
    handler:motor object
return:PANS_type
*/
PANS_type xp_pans_get_en_state(const PANS_handler *handler){
    return xp_pans_read_coil(handler,PANS_EN_STATE_RES);
}


/*
get motor run state 0x120
param:
    handler:motor object
return:PANS_type
*/
PANS_type xp_pans_get_stb120_state(const PANS_handler *handler){
    return xp_pans_read_coil(handler,0x120);
}

/*
get motor run state
param:
    handler:motor object
return:PANS_type
*/
PANS_type xp_pans_get_run_state(const PANS_handler *handler){
    return xp_pans_read_coil(handler,PANS_RUN_STATE_RES);
}


/*
get motor warn state
param:
    handler:motor object
return:PANS_type
*/
PANS_type xp_pans_get_warn_state(const PANS_handler *handler){
    return xp_pans_read_coil(handler,0xA1);
}

/*
get motor buzy state
param:
    handler:motor object
return:PANS_type
*/
PANS_type xp_pans_get_buzy_state(const PANS_handler *handler){
    return xp_pans_read_coil(handler,0x140);
}

/*
get motor read en state
*/
PANS_type xp_pans_get_en_data(const PANS_handler *handler){
    return xp_pans_read_coil(handler,0x60);
}


/****************************holding read/write**********************/
/*
pansonic motor write reginster
param:
    handler:motor object
    addr：reginster address
    data:write data
    len:write length
return:
    0=success
    -1=modbus return error
*/
static int xp_pans_write_reginster(const PANS_handler *handler,u16 addr,u16 *data,u8 len){
    int sta=0;
    MB_data modbus;
    u8 rec_data[10];
    u8 rec_len;
    u8 count=5;

    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_w;
    modbus.start_addr=addr;
    modbus.wBuf=data;
    modbus.rwLen=len;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--){
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta!=0)return -1;
    return 0;
}

/*
pansonic motor read reginster
param:
    handler:motor object
    addr:reginster address
    rec_data:save recive data
    rec_len:save recive length
return:
    0=success
    -1=modbus return error
*/
static int xp_pans_read_reginster(const PANS_handler *handler,u16 addr,u16 read_len,u8 *rec_data,u8 *rec_len){
    int sta=0;
    MB_data modbus;
    u8 count=5;


    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_r;
    modbus.start_addr=addr;
    modbus.rwLen=read_len;
    modbus.resBuf=rec_data;
    modbus.resLen=rec_len;
    while(count--){
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta!=0)return -1;
}

/*
pansonic clear location
param:
    handler:motor object
return:
    0=success
    -1=modbus return error
*/
int xp_pans_clear_location(const PANS_handler *handler){
    u16 data=0x6165;
    return xp_pans_write_reginster(handler,0x4100,&data,1);
}

/*
clear pansonic motor warn
param:
    handler:motor object
return:
    0=success
    -1=modbus return error
*/
int xp_pans_clear_warn(const PANS_handler *handler){
    u16 data=0x7274;
    return xp_pans_write_reginster(handler,0x4102,&data,1);
}

/*
set pansonic motor block
param:
    handler:motor object
    block:block number
return:
    0=success
    -1=set error
*/
int xp_pans_set_block(const PANS_handler *handler,u16 block){
    u16 data=block;
    return xp_pans_write_reginster(handler,0x4414,&data,1);
}

/*
set panasonic motor home offset
param:
    handler:motor object
    offset:home offset value
return:
    0=success
    -1=set error
*/
int xp_pans_set_home(const PANS_handler *handler,u32 offset){
    u16 data[2];
    
    data[0]=offset;
    data[1]=offset>>16;
    return xp_pans_write_reginster(handler,0x4631,data,2);
}


/*
set panasonic motor max limit
param:
    handler:motor object
    max:max limit value
return:
    0=success
    -1=set error
*/
int xp_pans_set_max(const PANS_handler *handler,u32 max){
    u16 data[2];

    data[0]=max;
    data[1]=max>>16;
    return xp_pans_write_reginster(handler,0x4633,data,2);
}

/*
set pansonic motor min limit
param:
    handler:motor object
    min:min value
return:
    0=success
    -1=set error
*/
int xp_pans_set_min_limit(const PANS_handler *handler,u32 min){
    u16 data[2];

    data[0]=min;
    data[1]=min>>16;
    return xp_pans_write_reginster(handler,0x4635,data,2);
}

/*
get pansonic actual speed
param
    handler:motor object
return:speed value
*/
int xp_pans_get_actual_speed(const PANS_handler *handler){
    u8 rec_data[10];
    u8 rec_len;
    int sta=0;

    sta=xp_pans_read_reginster(handler,0x4d06,2,rec_data,&rec_len);
    if(sta!=0){
        println("pansonic motor get actual speed error!");
        return 10000;
    }

    return (rec_data[2]<<24|rec_data[3]<<16|rec_data[0]<<8|rec_data[1]);
}

/*
get pansonic motor encode value
param:
    handler:motor object
return:
    0=success

*/
int xp_pans_get_encode(const PANS_handler *handler,PANS_encode *encode){
    int sta=0;
    u8 rec_data[20];
    u8 rec_len;
    PANS_encode ec={0};
    u8 count=0;

    while(1){
        count++;
        if(count>5)break;
        sta=xp_pans_read_reginster(handler,0x4d24,2,rec_data,&rec_len);
        if(sta!=0)continue;
        ec.p=rec_data[2]<<24|rec_data[3]<<16|rec_data[0]<<8|rec_data[1];
        sta=xp_pans_read_reginster(handler,0x4d28,2,rec_data,&rec_len);
        if(sta!=0)continue;
        ec.r=rec_data[2]<<24|rec_data[3]<<16|rec_data[0]<<8|rec_data[1];
        break;
    }
    if(count>5) return -1;
    
    encode->p=ec.p;
    encode->r=ec.r;
    encode->h=((uint64_t)ec.r << 23) | ec.p;
	//print("<%d %d %llu>\r\n",encode->r,encode->p, encode->h);    
	return 0;
}

/*
package block cmd
param:
    block:save block data
    cmd:cmd code
    speed:speed code
    acc:acc code
    dec:dec code
    dir:run direction
return:
    0=success
    -1=block pointer=null
    -2=cmd is not avalible
*/
int xp_pans_package_block(PANS_block *block,u8 cmd_code,u8 speed_code,u8 acc_code,u8 dec_code,s8 dir){
    u8 cmd[4]={0};

    if(block==NULL){
        println("error:xp_pans_package_block() block=NULL!");
        return 0;
    }
    if(cmd_code<1||cmd_code>0x0c){
        println("error: xp_pans_package_block() cmd is wrong,cmd=%d",cmd_code);
        return -2;
    }
    cmd[1]=(dec_code<<4)|(dir<<2);
    if(cmd_code==0x06){
        cmd[1]|=0x02;
    }
    else if(cmd_code==0x03){
        cmd[1]|=0x03;
    }
    else if(cmd_code==0x09){
        cmd[1]|=0x03;
    }
    else if(cmd_code==0x0a){
        cmd[1]|=0x02;
    }
    else if(cmd_code==0x05){
        cmd[1]|=0x03;
    }
    cmd[2]=(speed_code<<4)|acc_code;
    cmd[3]=cmd_code;
    memcpy(&block->cmd,cmd,4);
    return 0;
}


/*
package block with jump type
*/
int xp_pans_package_block_with_jump(PANS_block *block,u8 cmd_code,u8 speed_code,u8 acc_code,u8 dec_code,s8 dir,u8 jump_type){
    u8 cmd[4]={0};

    if(block==NULL){
        println("error:xp_pans_package_block() block=NULL!");
        return 0;
    }
    if(cmd_code<1||cmd_code>0x0c){
        println("error: xp_pans_package_block() cmd is wrong,cmd=%d",cmd_code);
        return -2;
    }
    cmd[1]=(dec_code<<4)|(dir<<2);
    cmd[1]|=jump_type;
    cmd[2]=(speed_code<<4)|acc_code;
    cmd[3]=cmd_code;
    memcpy(&block->cmd,cmd,4);
    return 0;
}

/*
config pansonic motor block
param:
    handler:motor object
    block:block data
return:
    0=success
    -1:set fail
*/
int xp_pans_config_block(const PANS_handler *handler,const u16 addr,const u32 block){
    int sta=0;
    MB_data modbus;
    u16 data[2];
    u32 buf=block;
    u8 rec_data[10];
    u8 rec_len;
    u8 count=5;

    memcpy(data,&buf,4);

    modbus.slave_id=handler->slave_id;
    modbus.mb_fun=mb_hold_w;
    modbus.start_addr=addr;
    modbus.wBuf=data;
    modbus.rwLen=2;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    while(count--){
        sta=xp_modbus_ask(handler->port,&modbus);
        if(sta==0)return 0;
    }
    if(sta!=0)return -1;
}



/*
panasonic motor modbus test
param:
    handler:motor object
return:
    0=success
    -1=error
    -2=return data error
*/
int xp_pans_modbus_test(const PANS_handler *handler){
    int sta=0;
    MB_data modbus;
    u16 data=0x1234,rec;
    u8 rec_data[2];
    u8 rec_len;

    modbus.slave_id=handler->slave_id;
    modbus.fun_code=0x08;
    modbus.start_addr=0;
    modbus.wBuf=&data;
    modbus.rwLen=1;
    modbus.resBuf=rec_data;
    modbus.resLen=&rec_len;
    sta=xp_modbus_ask(handler->port,&modbus);
    if(sta!=0){
        println("panasonic motor modbus offline,slave id=%d",handler->slave_id);
        return -1;
    }
    println("panasonic motor modbus test ,recive data:%d,%d",rec_data[0],rec_data[1]);
    
    rec=rec_data[0]|(rec_data[1]<<8);
    if(rec!=data){
        println("panasonic motor modbus test,recive data error,slave id=%d",handler->slave_id);
        return -2;
    }
    else{
        println("panasonic motor modbus online!");
        return 0;
    }
}


/*
config pansonic motor move block
param:
    handler:motor object
    speed_code:speed block number
    dis:move distance
return:
    0=success
*/
int xp_pans_config_move(PANS_handler *handler,const u8 speed_code,u8 jump_type,const u32 dis){
    
    if(dis==0)return 0;
    // if(xp_pans_package_block(&handler->block[block_rel],0x01,speed_code,speed_code,speed_code,0)!=0)return 1;
    if(xp_pans_package_block_with_jump(&handler->block[block_rel],0x01,speed_code,speed_code,speed_code,0,jump_type)!=0)return -1;
    if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+block_rel*4,handler->block[block_rel].cmd)!=0)return -2;
    handler->block[block_rel].data=dis;
    if(xp_pans_config_block(handler,PANS_BLOCK_DATA0_RES+block_rel*4,handler->block[block_rel].data)!=0)return -3;
    if(xp_pans_set_block(handler,block_rel)!=0)return -4;
    return 0;
}


/*
set motor move with speed
param:
    handler:motor object
    speed_code:speed block number
    dis:move distance
return:
    0=success
    -1=set error
*/
int xp_pans_move_s(PANS_handler *handler,const u8 speed_code,const u32 dis){
    if(handler==NULL)return -1;
    if(xp_pans_run(handler,pans_off)!=0)return -2;
    if(xp_pans_config_move(handler,speed_code,0,dis)!=0)return -3;
    if(xp_pans_run(handler,pans_on)!=0)return -4;
    return 0;
}


/*
set motor action a distance 
param:
    handler:motor object
    dis:move distance
return:
    0=success
    -1=set error
*/
int xp_pans_move(PANS_handler *handler,const u32 dis){
    int sta=0;

    if(handler==NULL)return -1;
    sta=xp_pans_run(handler,pans_off);
    if(sta<0)return -2;
    handler->block[block_rel].data=dis;
    sta=xp_pans_config_block(handler,PANS_BLOCK_DATA0_RES,handler->block[block_rel].data);
    if(sta!=0)return -3;
    sta=xp_pans_set_block(handler,block_rel);
    if(sta!=0)return -4;
    sta=xp_pans_run(handler,pans_on);
    if(sta<0)return -5;
    return 0;
}


/*
config motor round block
param:
    handler:motor object
    speed_code:speed block number
    dir:run dir
return:
    0=success
*/
int xp_pans_round_block(PANS_handler *handler,const u8 speed_code,const s8 dir){
    if(xp_pans_package_block(&handler->block[block_round],0x03,speed_code,speed_code,speed_code,dir)!=0)return -2;
    if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+block_round*4,handler->block[block_round].cmd)!=0)return -3;
    return 0;
}

/*
config motor round
param:
    handler:motor object
    speed_code:speed block number
    dir:run dir,0=正转，1=反转
return:
    0=success
*/
int xp_pans_confg_round(PANS_handler *handler,const u8 speed_code,const s8 dir){
    if(xp_pans_round_block(handler,speed_code,dir)!=0)return -1;
    return 0;
}


/*
set motor round aways
param:
    handler:motor object
    speed_code:speed block number
    dir:run dir,0=正转，1=反转
return:
    0=success
    other=set fail
*/
int xp_pans_round(PANS_handler *handler,const u8 speed_code,const s8 dir){
    if(xp_pans_run(handler,pans_off)<0)return -1;
    if(xp_pans_confg_round(handler,speed_code,dir)!=0)return -2;
    if(xp_pans_update_block(handler,speed_code,dir)!=0)return -3;
    if(xp_pans_set_block(handler,block_round)!=0)return -4;
    if(xp_pans_run(handler,pans_on)<0)return -5;
    return 0;
}



/*
update speed
param:
    handler:motor object
    speed_code:speed number
    dir:run dir
return:
    0=success   
*/
int xp_pans_update_block(PANS_handler *handler,const u8 speed_code,const s8 dir){
    if(xp_pans_package_block(&handler->block[block_update_speed],0x06,speed_code,0,0,dir)!=0)return -2;
    if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+block_update_speed*4,handler->block[block_update_speed].cmd)!=0)return -3;
    return 0;
}

/*
event touch things
param:
    handler:motor object
    target:compare target.7=si4 input
    block:target block
    data:比较阀值
return:
    0=success
*/
int xp_pans_compare_block(PANS_handler *handler,u8 save,u8 target,u8 block,u32 data){
    if(xp_pans_package_block(&handler->block[block_event],0x0a,target,0,0,block)!=0)return -2;
    if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+save*4,handler->block[block_event].cmd)!=0)return -3;
    if(xp_pans_config_block(handler,PANS_BLOCK_DATA0_RES+save*4,data)!=0)return -3;
    return 0;
}


/*
pansonic motor jump block run
param：
    handler:motor object
    save_block:block save number
    target_block:target block number,<64
return:
    0=success
*/
int xp_pans_jump_block(PANS_handler *handler,u8 save_block,u8 target_block){
    if(xp_pans_package_block(&handler->block[block_jump],0x09,0,0,0,target_block)!=0)return -2;
    if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+save_block*4,handler->block[block_jump].cmd)!=0)return -3;
    return 0;
}

/*
set pansonic motor speed on speed block 0
param:
    speed:speed value
return:
    0=success
*/
int xp_pans_change_speed(PANS_handler *handler,u8 speed_block,u16 speed){
#if 1
    u16 data=speed;

    // if(xp_pans_run(handler,pans_off)<0)return -1;
    if(xp_pans_write_reginster(handler,0x4600+speed_block,&data,1)!=0)return -2;
    // if(xp_pans_run(handler,pans_on)<0)return -3;
#else
	if(xp_pans_write_reginster(handler,0x4600,&speed,1)!=0)return -2;
	uint32_t temp = 0;
	uint8_t cmd[4] = {0};
	cmd[1] = (0 << 4) | (0 << 2);
	cmd[1] |= 0;
	cmd[2] = (0 << 4) | 0;
	cmd[3] = 0x02;
	memcpy(&temp, cmd, 4);
	if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES,&temp,1)!=0)return -2;
	if(xp_pans_run(handler,pans_off)<0)return -1;
	xp_pans_set_block(handler, 0);
	if(xp_pans_run(handler,pans_on)<0)return -3;
#endif
    return 0;
}


/*
config stop block
param:
    handler:motor object
    type:stop type,0=slow stop,1=fast stop
return:
    0=success
*/
int xp_pans_stop_block(PANS_handler *handler,u8 save,u8 type){
    // xp_pans_run(handler,pans_off);
    if(xp_pans_package_block(&handler->block[block_stop],0x05,type,0,0,0)!=0)return -2;
    if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+save*4,handler->block[block_stop].cmd)!=0)return -3;
    // if(xp_pans_set_block(handler,block_stop)!=0)return -4;
}


/*
config motor driver block
config 5 block
param:
    handler:motor object
return:
    0=success
    -1=set fail
*/
int xp_pans_init_block(PANS_handler *handler){
    int sta=0;

    if(xp_pans_run(handler,pans_off)<0)return -1;
    for(u8 i=0;i<5;i++){
        if(xp_pans_package_block(&handler->block[i],i+1,0,0,0,0)!=0){
            sta=-4;
            break;
        }
        if(xp_pans_config_block(handler,PANS_BLOCK_CMD0_RES+i*2,handler->block[i].cmd)!=0){
            sta=-2;
            break;
        }
        if(xp_pans_config_block(handler,PANS_BLOCK_DATA0_RES+i*2,handler->block[i].data)!=0){
            sta=-3;
            break;
        }
    }
    if(sta!=0)return sta;
    sta=xp_pans_save_setting(handler);
    if(sta!=0)return -5;
    // if(xp_pans_run(handler,pans_on)<0)return -4;
    return 0;
}

/*
config motor acc speed dec speed,5 block
param:
    handle:motor object
return:
    0=success
*/
int xp_pans_config_speed(const PANS_handler *handler){
    u16 data;

    for(u8 i=0;i<2;i++){
        data=handler->config.speed[i];
        if(xp_pans_write_reginster(handler,0x4600+i,&data,1)!=0)return -1;
        data=handler->config.acc[i];
        if(xp_pans_write_reginster(handler,0x4610+i,&data,1)!=0)return -2;
        data=handler->config.dec[i];
        if(xp_pans_write_reginster(handler,0x4620+i,&data,1)!=0)return -3;
    }
    return 0;
}

/*
save setting to eeprom
param:
    handler:motor object
return:
    0=success
*/
int xp_pans_save_setting(const PANS_handler *handler){
    u16 data[2]={0,0x6173};
    return xp_pans_write_reginster(handler,0x1020,data,2);
}


/*
keep synic
param:
    motor0:motor object
    motor1:motor object
return:
    0=synic
    other=dispart pluse
*/
int xp_pans_keep_synic(const PANS_handler *motor0,const PANS_handler *motor1,u32 dispartMax){
    PANS_encode encode[2]={0};
    u32 dis=0;

    if(xp_pans_get_encode(motor0,&encode[0])!=0)return -1;
    if(xp_pans_get_encode(motor1,&encode[1])!=0)return -2;
    dis=abs((int)((encode[0].h-encode[1].h)/838.8608));

    if(dis>dispartMax){
        return dis;
    }
    return 0;
}


/*
get motor error code
param:
    motor:motor object
return: error code
*/
u16 xp_pans_get_error_code(const PANS_handler *motor){
    u8 rec_buf[20]={0};
    u8 rec_len=0;

    if(xp_pans_read_reginster(motor,0x4001,1,rec_buf,&rec_len)!=0)return 0;
    println("motor %d,error code: %d.%d",motor->slave_id,rec_buf[0],rec_buf[1]);
    return (rec_buf[0]<<8|rec_buf[1]);
}



/*
set acc and dec
param:
    motor:motor handler
    acc: acc value
    dec: dec value
    block:block num
return:
    0=success
*/
int xp_pans_set_acc(PANS_handler *motor,u16 acc,u16 dec,u8 block){
    u16 data=acc;
    if(data!=0){
        motor->config.acc[block]=data;
        if(xp_pans_write_reginster(motor,0x4610+block,&data,1)!=0)return -1;
    }
    data=dec;
    if(data!=0){
        motor->config.dec[block]=data;
        if(xp_pans_write_reginster(motor,0x4620+block,&data,1)!=0)return -2;
    }
    return 0;
}




//end of the file
